from matplotlib import pyplot as plt
from Algorithm.AlgorithmModel import ControlResult, ControlMode
from Algorithm.M_BaseServo import BaseServo
from Algorithm.AdmittanceRoateControl import AddmittanceControl
from Algorithm.AdaptiveAdmittanceControl import AdaptiveAdmittanceControl
from Algorithm.M_PIForceController import PIForceController
import numpy as np
import cv2

from Common.ControlEnum import forcemode
from Common.MatrixProcess import trans_pose
class my_test(BaseServo):
    def __init__(self,initPose,dt=0.002,mode=forcemode.FreeMode):
        super(my_test,self).__init__(ControlMode=ControlMode.servoL)
        print(self.result)
        self.test_path(initPose,step=dt)
        self.adjpose=[0,0,0,0,0,0]#力控调整项
        self.mode=mode
        self.controller=AddmittanceControl(m3=np.array([5,5,2]),
                                           b3=np.array([30,30,10]),
                                           k3=np.array([0,0,0]),
                                           f3=np.array([0,0,0]),
                                           dt=0.002,
                                           teachmode=mode)

        None

    def test_path(self,initPose,step=0.002,path=0.2,target_v =0,target_a = 0):
        """
        螺旋寻孔,最大圆半径=incrR*num_turns * 2 * np.pi
        Args:
            initPose:当前机器人位置
            startR:起始R
            incrR:每个时刻半径增量，每圈增量=incrR*points,大圆直径=incrR*points/10mm
            num_turns:多少圈
            points:每圈多少个点
        Returns:
        """
        self.initPose = initPose
        # self.z = np.arange(self.initPose[2],self.initPose[2]-path,-step)#速度一定
        # self.maxcount = self.z.size
        self.maxcount = 3000
        self.z = np.zeros(self.maxcount)
        self.target_v = target_v  # v = 0.01m/s
        i=0
        while(i<self.maxcount):
            # self.z[i] = self.initPose[2] - step*step*i*i*target_a      #根据加速度设置xin,x=1/2* a *t^2
            self.z[i] = self.initPose[2] - step * i * self.target_v - step*step*i*i*target_a    #step为ur5e的控制周期 2ms
            i=i+1
        self._curPoseCount = 0

        # self.maxcount=20000

    def Move(self, **kwargs):
        cur_pose= kwargs['actualTcpPose']
        #获取pose
        cur_vel = kwargs['actualTcpSpeed']
        #获取force
        cur_force=kwargs['force']
        #print("cur_force: ",cur_force)
        #计算轨迹
        #获取目标轨迹
        pose=self.initPose
        pose[2]=self.z[self._curPoseCount]
        self._curPoseCount+=1
        # print(self._curPoseCount,pose[:3])
        #if(np.linalg.norm(cur_force)>3):
        pose=self.controller.calpose(cur_pose,pose,cur_force)

        # print("calpose=", pose[:3],"cur: ",cur_pose[:3])

        self.result['ServoParm'][0] = pose

        if(self._curPoseCount==self.maxcount-1
           #       or
           # abs(np.linalg.norm(cur_force))>=100
        ):
            self.result['ControlResult'] = ControlResult.Finish
            if(self.mode==forcemode.TeachMode):
                teachforce=self.controller.get_save_value()
                np.savetxt("demoforce.txt", teachforce)

        elif(np.linalg.norm(cur_force)>80):
            self.result['ControlResult'] = ControlResult.Finish
            print("超出最大力阈值",cur_force)
        else:
            self.result['ControlResult'] = ControlResult.Running
        return self.result

